#include<iostream>
#include<stdlib.h>
#include<unistd.h>
#include<thread>
#include"cameraStart.h"
#include"video.h"
#include<opencv2/opencv.hpp>
using namespace std;

bool CameraStart::ConectCameras()  //开始连接相机
{
    Video::CameraParams camerapareams;
    while(true){
        switch (ENTER) {
        case 0:
             if(camera.videoCheck()) //搜索相机
             {
                 ENTER = 1;  //搜索相机成功则进入初始化相机
                 continue;
             }
             if(VideoCheck_LOSTUM>=VideoCheck_NUM)
             {
                 exit(-1); //搜索相机失败的次数达到设定值则，强制退出
                 return false;
             }
             VideoCheck_LOSTUM++;
            break;
       case 1:
             if(camera.videoOpen())//初始化相机
             {
                 Video::ETrigType type = Video::ETrigType::trigContinous;//设置触发模式 连续拉流
                 camera.CameraChangeTrig(type);
                 ENTER = 2;
                 continue;
             }
             if(VideoOpen_LOSTUM>=VideoOpen_NUM)
             {
                 exit(-1);//初始化相机失败次数达到设定值，强制退出
                 return false;
             }
             VideoOpen_LOSTUM++;
            break;
      case 2:
            if(camera.videoStart())//创建流对象
            {
                camera.videoStopStream();//断开拉流
                camerapareams = camera.query_params(); //查询相机内部参数
                ENTER = 3;
                continue;
            }
            return false;
      case 3:
               //设置曝光                                   //设置增益                     //设置分辨率                //设置亮度
            if(camera.SetExposeTime(8000.0)&&camera.SetAdjustPlus(2.0)&&camera.setResolution(Size(1280,1024))&&camera.setBrightness(60))
            {
                camera.saveConfiguration(0);  //保存相机参数
                camerapareams = camera.query_params();  //再次查询相机内部参数
                return true;
            }
            return false;
        default:
            return false;
        }
}
}

void CameraStart::CameraGetStream(shared_ptr<CameraStart> cameraStart) //开始拉流
{
    if(ConectCameras())
    {
         camera.startGrabbing();  //开始拉流
         thread work1(bind(&CameraStart::CameragetImage,this));      //取图线程
        // thread work2(bind(&CameraStart::CameraImageProcess,this));  //处理线程

          rclcpp::spin(cameraStart);
          work1.join();
         // work2.join();

    }
}

void CameraStart::CameragetImage()  //取帧
{
    this->start=log.get_time_point(); //获取当前时钟
    while(true){
        camera.getFrame(src);  //获取一帧片
        if(!src.empty())
        {
            this->total_time=log.calculation_time_difference(this->start);  //计算与当前时间差
            fps = double(flag_imag)/total_time;    //计算帧率
            cout<<"No."<<flag_imag<<"  total time: "<<total_time<<"  "<<fps<<endl;// 显示帧率

             work1_flag = true;  //处理线程开始的标志
             flag_imag++;
            
            new_src = src.clone();
            work2_flag = true;  //获取到图片的标志         
   
        }
    }
}



void CameraStart::CameraImageProcess()  //处理帧
{
  while(!work1_flag){};
  while(true)
  {
      while(!work2_flag){};
      imshow("read_src",new_src); //显示图片
      waitKey(1);
      usleep(2000);
  }

}


 void CameraStart::timer_callback()  
 {

    auto img = sensor_msgs::msg::Image();

    img.header.stamp = this->now();
    img.header.frame_id = "camera_frame"; 
    img.encoding = "rgb8";

    img.height = new_src.rows;
    img.width = new_src.cols;

    img.is_bigendian = false;
    img.step = new_src.cols * new_src.elemSize(); // 计算图像的步长
    size_t size = new_src.total() * new_src.elemSize();
    img.data.resize(size);
    memcpy(&img.data[0], new_src.data, size);

    pub_img->publish(img);

 }